#include "../../include/robot/RobotController.h"
#include "../../include/robot/AlignAction.h"
#include "../../include/robot/finishingGroupAction.h"
#include "../../include/robot/targetFarGroupAction.h"
#include "../../include/robot/targetNearGroupAction.h"
using namespace std;
/*hmin 170 hmax 180 smin 100 smax 255 vmin 100 vmax 255 */

RobotController::RobotController(ArRobot* robot){
	this->robot = robot;
	align = false;
	threshold = 2400;
	th_ball =1000;
	controllerV = new VisionController();
	angolo=0;
	myState = SCANNING;
	myOldState = INIT;

}

RobotController::~RobotController(){
	delete(finishing);
	delete(targetfar);
	delete(targetnear);
	delete(scanning);
}
void RobotController::run(){
	pthread_create(&thread,0,&RobotController::start,this);
};

void RobotController::stop(){
	pthread_join(thread,0);
};

void RobotController::dowork()
{
	bool sca = true;
	bool far = true;
	float angoloPalla;
	controllerV->alza();
	while(true){
		controllerV->elaborateImage();

		float goalDepth = controllerV->getGoalDepth();
		float ballDepth = controllerV->getBallDepth();
		if(goalDepth==-1.0f && sca==true){
			myState = SCANNING;
		}else{
			angolo = controllerV->getBallAngle();
			if(goalDepth > threshold && far==true){
				myState = TARGETFAR;
			}else if((goalDepth<= threshold && goalDepth>1) || myOldState==TARGETNEAR
					|| myOldState==ALIGN){

				myState = TARGETNEAR;
				sca=false;
				far=false;
				if (ballDepth!=1.0f || align==false){
					angoloPalla = controllerV->getBallAngle();
					if(isInRangeFinal(angoloPalla,angolo,3)){
						myState=FINISHING;
					}else{
						myState= ALIGN;
					}
				}
			}
		}

		if(myState==SCANNING && myOldState!=SCANNING){
			cout << "SONO NELLO STATO SCANNING\n";
			scanning = new ScanningGroupAction(robot);
			scanning->activateExclusive();
			myOldState = SCANNING;
		}else if(myState==TARGETFAR && myOldState!=TARGETFAR){
			cout << "SONO NELLO STATO FAR\n";
			targetfar= new TargetFarGroupAction(robot,controllerV,goalDepth,angolo);
			targetfar->activateExclusive();
			myOldState = TARGETFAR;
		}else if(myState==TARGETNEAR && myOldState!=TARGETNEAR){
			controllerV->abbassa();
			targetnear= new TargetNearGroupAction(robot,angolo);
			cout << "SONO NELLO STATO TARGETNEAR\n";
			targetnear->activateExclusive();
			myOldState = TARGETNEAR;
		}else if(myState==ALIGN && myOldState!=ALIGN){
			cout << "MI ALLINEO ALLA PALLA\n";
			robot->setVel(0);
			AlignAction *al = new AlignAction(controllerV->getBallAngle(),controllerV);
			robot->addAction(al,100);
			myOldState=ALIGN;
		}else if (myState==FINISHING && myOldState!=FINISHING){
			cout<<"SONO NELLO STATO FINISHING\n";
			finishing= new FinishingGroupAction(robot);
			finishing->activateExclusive();
			myOldState= FINISHING;
		}
	}
}
void RobotController::dowork_final(){


}
bool RobotController::isInRangeFinal(float angolo1,float angolo2,float range){
	return ((angolo1>=-range && angolo1<=range) && (angolo2>=-range && angolo2<=range));
}
